# -*- coding: utf-8 -*-
import sys
import signal
from functools import partial
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QVBoxLayout
from PyQt5 import uic
from PyQt5.QtCore import Qt, QCoreApplication, QEvent, QRectF
from PyQt5.QtGui import QKeyEvent, QPainter, QColor, QFont, QPen, QLinearGradient, QTextOption
import tkinter as tk
from tkinter import messagebox

import subprocess
import time
from client_ import ROS_Client
from threading import Thread
from rviz_ import rvizWindow
from nav_new import NavGoalTableView


class MainWindow(QMainWindow):
    
    def __init__(self):
        super().__init__()
        self.host = '192.168.1.252'
        self.port = 8888
        self.setupUI()
        self.initUI()
        self.initSignals()
        self.dashBoard_show()
        self.mapNavigate_show()

    def show_popup(self, title, message):
        root = tk.Tk()
        root.withdraw()
        messagebox.showinfo(title, message)
        root.destroy()

    def setupUI(self):
        # ui_file = "C:\\Users\zuto\Desktop\zbot3_ws\src\zbot_monitor\scripts\mainwindow.ui"
        ui_file = "./ui/mainwindow.ui"
        self.ui = uic.loadUi(ui_file, self)
        self.setWindowTitle("ZBot Monitor")

    def initUI(self):
        self.frame_dashBoard = self.ui.frame_dashBoard
        self.frame_middle = self.ui.frame_middle
        self.page_navM = self.ui.page_navM
        # self.page_navM_Frame = self.ui.page_navM_Frame
        self.page_navM_Point1 = self.ui.page_navM_Point1
        self.pushButton_carNav = self.ui.pushButton_carNav
        self.pushButton_pointA = self.ui.pushButton_pointA
        self.pushButton_pointB = self.ui.pushButton_pointB

        self.pushButton_manageMaps = self.ui.pushButton_manageMaps
        self.pushButton_saveMap = self.ui.pushButton_saveMap

        self.pushButton_mapping2D = self.ui.pushButton_mapping2D


        self.lineEdit_ip = self.ui.lineEdit_ip
        self.lineEdit_port = self.ui.lineEdit_port
        self.lineEdit_ip.setText(self.host)
        self.lineEdit_port.setText(str(self.port))

        self.pushButton_conn = self.ui.pushButton_conn
        self.pushButton_disconnect = self.ui.pushButton_disconnect

        self.buttons = {
            1: self.ui.pushButton_carManage,
            2: self.ui.pushButton_sensorManage,
            3: self.ui.pushButton_manageMaps,
            4: self.ui.pushButton_navigateManage,
            5: self.ui.pushButton_robotManage
        }


        self.direct_buttons = {
            1: self.ui.pushButton_up_left,
            2: self.ui.pushButton_up,
            3: self.ui.pushButton_up_right,
            4: self.ui.pushButton_left,
            5: self.ui.pushButton_right,
            6: self.ui.pushButton_down_left,
            7: self.ui.pushButton_down,
            8: self.ui.pushButton_down_right
        }

        self.robot_control_buttons = {
            1:self.ui.pushButton_J1_add,
            2:self.ui.pushButton_J1_min,
            3:self.ui.pushButton_J2_add,
            4:self.ui.pushButton_J2_min,
            5:self.ui.pushButton_J3_add,
            6:self.ui.pushButton_J3_min,
            7:self.ui.pushButton_J4_add,
            8:self.ui.pushButton_J4_min,
            9:self.ui.pushButton_J5_add,
            10:self.ui.pushButton_J5_min,
            11:self.ui.pushButton_J6_add,
            12:self.ui.pushButton_J6_min
        }

        self.robot_joint_label = {
            1: self.label_robot_J1,
            2: self.label_robot_J2,
            3: self.label_robot_J3,
            4: self.label_robot_J4,
            5: self.label_robot_J5,
            6: self.label_robot_J6
        }

        self.pushButton_robotHome = self.ui.pushButton_robotHome
        self.pushButton_robotStop = self.ui.pushButton_robotStop
        self.pushButton_robotZero = self.ui.pushButton_robotZero
        self.label_robotType = self.ui.label_robotType
        self.label_robotStatus = self.ui.label_robotStatus
        
        self.stackedWidget = self.ui.stackedWidget
        self.stackedWidget_config = self.ui.stackedWidget_config

        self.car_control_buttons = {
            'start': self.ui.pushButton_carStart,
            'stop': self.ui.pushButton_carStop
        }

    def initSignals(self):
        for index, button in self.buttons.items():
            button.clicked.connect(partial(self.on_pushButton_Manage_clicked, index))

        self.car_control_buttons['start'].clicked.connect(self.on_pushButton_carStart_clicked)
        self.car_control_buttons['stop'].clicked.connect(self.on_pushButton_carStop_clicked)

        for index, button in self.robot_control_buttons.items():
            button.clicked.connect(partial(self.on_pushButton_robotControl_clicked, index))

        for index, button in self.direct_buttons.items():
            button.clicked.connect(partial(self.on_pushButton_direct_clicked, index))

        for index, label in self.robot_joint_label.items():
            label.setText(str(0))
            label.setAlignment(Qt.AlignCenter)
        
        self.label_robotType.setText("KT-6")
        self.label_robotStatus.setText("Offline")
        self.pushButton_robotStop.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=21))
        self.pushButton_robotHome.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=22))
        self.pushButton_robotZero.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=23))


        self.pushButton_conn.clicked.connect(self.on_pushButton_connect_clicked)
        self.pushButton_disconnect.clicked.connect(self.on_pushButton_disconnect_clicked)

        # self.pushButton_manageMaps.clicked.connect(self.on_pushButton_mapManage_clicked)
        self.pushButton_mapping2D.clicked.connect(self.on_pushButton_mapManage_clicked)
        self.pushButton_saveMap.clicked.connect(self.on_pushButton_saveMaps_clicked)

        self.pushButton_carNav.clicked.connect(self.on_pushButton_carNav_clicked)
        self.pushButton_pointA.clicked.connect(self.on_pushButton_points_clicked)

    def set_button_style(self, button, active):
        style = "background-color: rgb(115, 152, 163);" if active else ""
        button.setStyleSheet(style)

    def dashBoard_show(self):
        try:
            self.dashboard = Dashboard(self.frame_dashBoard)
            self.dashboard.setGeometry(0,0,200,378)
            self.dashboard.show()

            set_parameters_thread = Thread(target=self.set_rosBridge_parameters)
            set_parameters_thread.start()

        except Exception as e:
            print(f"dashBoard_show error: {e}")

    def mapNavigate_show(self):
        try:
            self.mapNavigate = rvizWindow(self.frame_middle)
            self.mapNavigate.setGeometry(0,0,730,760)
            self.mapNavigate.show()

        except Exception as e:
            print(f"mapNavigate_show error: {e}")

    def navGoal_show(self):
        try:
            # if not hasattr(self, 'navGoal') or self.navGoal is None:
            #     self.navGoal = NavGoalTableView(self.page_navM)
            #     # 使用布局管理器来管理 NavGoalTableView 的位置和大小
            #     layout = QVBoxLayout(self.page_navM)
            #     layout.addWidget(self.navGoal)
            # else:
            #     self.navGoal.show()

            self.navGoal = NavGoalTableView(self.page_navM_Point1)
            self.navGoal.setGeometry(0,0,300,760)
            self.navGoal.show()

        except Exception as e:
            print(f"navGoal_show error: {e}")

    def set_rosBridge_parameters(self):
        try:
            if self.ros_client.client_connected:
                speed = self.ros_client.ros_speed
                temperature = 45
                battery_level = self.ros_client.ros_battery
                if self.ros_client.ros_angle >= 0 and self.ros_client.ros_angle <= 180:
                    gear = "D"
                elif self.ros_client.ros_angle < 0 and self.ros_client.ros_angle >= -180:
                    gear = "R"
                else:
                    gear = "N"

            else:
                speed = 0
                temperature = 45
                battery_level = 80
                gear = "N"
            
            self.dashboard.set_speed(speed)
            self.dashboard.set_temperature(temperature)
            self.dashboard.set_battery_level(battery_level)
            self.dashboard.set_gear(gear)
        
        except Exception as e:
            print(f"set_rosBridge_parameters error: {e}")

    def start_mobaxterm_vnc_session(self, host, port=5901):
        # MobaXterm可执行文件路径，假设它安装在默认位置
        mobaxterm_path = r'"D:\Programs\Mobatek\MobaXterm\MobaXterm.exe"'
        
        # 构建命令字符串
        command = f'{mobaxterm_path} -vnc -host {host} -port {port}'
        
        try:
            # 使用subprocess.Popen启动MobaXterm VNC会话
            process = subprocess.Popen(command, shell=True)
            print(f"Starting VNC session with host: {host}, port: {port}")
            
            # 等待进程结束，这里可以根据需要选择是否等待
            # process.wait()
            
        except Exception as e:
            print(f"An error occurred: {e}")

    def on_pushButton_mapManage_clicked(self):
        try:
            self.start_mobaxterm_vnc_session('192.168.1.252', 5901)
            time.sleep(1)
            if self.ros_client:
                self.ros_client.send_command(ros_run="rviz")
            else:
                print(f"No ROS client connect")
                pass

        except Exception as e:
            print(f"on_pushButton_mapManage_clicked error: {e}")

    ### 保存地图
    def on_pushButton_saveMaps_clicked(self):
        try:
            if self.ros_client:
                self.ros_client.send_command(ros_run="saveMap")
                self.show_popup("地图管理", "保存地图成功")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_saveMap_clicked error: {e}")

    def on_pushButton_Manage_clicked(self, index):
        try:
            for i, button in self.buttons.items():
                self.set_button_style(button, i == index)
            self.stackedWidget.setCurrentIndex(index - 1)
            self.stackedWidget_config.setCurrentIndex(index - 1)

        except Exception as e:
            print(f"on_pushButton_Manage_clicked error: {e}")

    ### 小车导航参数配置
    def on_pushButton_carNav_clicked(self):
        try:
            if self.ros_client:
                self.ros_client.send_command(ros_run="carNavConfig")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_carNav_clicked error: {e}")

    ### 小车定点导航按钮响应
    def on_pushButton_points_clicked(self):
        try:
            if self.ros_client:
                self.ros_client.send_command(ros_run="pointA")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        except Exception as e:
            print(f"on_pushButton_pointA_clicked error: {e}")

    ### 小车启动按钮响应
    def on_pushButton_carStart_clicked(self):
        try:
            self.car_control_buttons['start'].setEnabled(False)
            self.car_control_buttons['stop'].setEnabled(True)
            launch_file = 'zbot3_bringup.launch'
            bag_name = "zbot3_drive"
            self.ros_client.send_command(ros_launch = "roslaunch", ros_bag = bag_name, data = launch_file)
        
        except Exception as e:
            print(f"on_pushButton_carStart_clicked error: {e}")
    
    ### 小车停止按钮响应
    def on_pushButton_carStop_clicked(self):
        self.car_control_buttons['start'].setEnabled(True)
        self.car_control_buttons['stop'].setEnabled(False)
        # self.car_thread_2 = Thread(target=RosBridge.stopRos)
        # self.car_thread_2.start()

    ### 小车方向控制按钮响应
    def on_pushButton_direct_clicked(self, index):
        try:
            key_map = {
                1: (Qt.Key_U, Qt.NoModifier, 'u'),
                2: (Qt.Key_I, Qt.NoModifier, 'i'),
                3: (Qt.Key_O, Qt.NoModifier, 'o'),
                4: (Qt.Key_J, Qt.NoModifier, 'j'),
                5: (Qt.Key_L, Qt.NoModifier, 'l'),
                6: (Qt.Key_M, Qt.NoModifier, 'm'),
                7: (Qt.Key_Comma, Qt.ShiftModifier, ','),
                8: (Qt.Key_Period, Qt.ShiftModifier, '.'),
            }

            if index in key_map:
                self.send_key_event(*key_map[index])

        except Exception as e:
            print(f"on_pushButton_direct_clicked error: {e}")

    def send_key_event(self, key, modifiers, text):
        try:
            press_event = QKeyEvent(QEvent.KeyPress, key, modifiers, text)
            release_event = QKeyEvent(QEvent.KeyRelease, key, modifiers)

            QCoreApplication.postEvent(self.focusWidget(), press_event)
            QCoreApplication.postEvent(self.focusWidget(), release_event)
        
        except Exception as e:
            print(f"send_key_event error: {e}")
    
    ### 机械手控制响应
    def on_pushButton_robotControl_clicked(self, index):
        try:
            if index == 1:
                # gcode = 'M21 G91 G1 X10 F1000'
                gcode = "gcode: 'M21 G91 G1 X10 F1000'"
            elif index == 2:
                # gcode = 'M21 G91 G1 X-10 F1000'
                gcode = "gcode: 'M21 G91 G1 X-10 F1000'"
            elif index == 3:
                # gcode = 'M21 G91 G1 Y10 F1000'
                gcode = "gcode: 'M21 G91 G1 Y10 F1000'"
            elif index == 4:
                # gcode = 'M21 G91 G1 Y-10 F1000'
                gcode = "gcode: 'M21 G91 G1 Y-10 F1000'"
            elif index == 5:
                # gcode = 'M21 G91 G1 Z10 F1000'
                gcode = "gcode: 'M21 G91 G1 Z10 F1000'"
            elif index == 6:
                # gcode = 'M21 G91 G1 Z-10 F1000'
                gcode = "gcode: 'M21 G91 G1 Z-10 F1000'"
            elif index == 7:
                gcode = "gcode: 'M21 G91 G1 A10 F1000'"
            elif index == 8:
                gcode = "gcode: 'M21 G91 G1 A-10 F1000'"
            elif index == 9:
                gcode = "gcode: 'M21 G91 G1 B10 F1000'"
            elif index == 10:
                gcode = "gcode: 'M21 G91 G1 B-10 F1000'"
            elif index == 11:
                gcode = "gcode: 'M21 G91 G1 C10 F1000'"
            elif index == 12:  
                gcode = "gcode: 'M21 G91 G1 C-10 F1000'"
            elif index == 21:
                gcode = "gcode: 'o117'"
            elif index == 22:
                gcode = "gcode: '\$h'"
            elif index == 23:
                gcode = "gcode: 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'"
            
            #### 修改为调用本地service控制mirobot
            self.ros_client.send_command(ros_run="rosrun",ros_gcode="GCode", data=gcode)
            # self.ros_server.send_data_process(gcode)
            # rosservice.call('/MirobotServer/SetGcodeCmd', gcode) 

        except Exception as e:
            self.show_popup("错误提示", "机械手控制失败原因:未连接")
            print(f"on_pushButton_robotControl_clicked error: {e}")

    ### TCP连接按钮响应
    def on_pushButton_connect_clicked(self):
        try:
            self.host = self.lineEdit_ip.text()
            self.port = self.lineEdit_port.text()

            if not self.port or not self.port.isdigit():
                raise ValueError("Port must be a non-empty numeric value.")
            port = int(self.port)
                    
            # Optionally, validate that the port number is within the valid range for TCP/UDP ports
            if not (0 < port <= 65535):
                raise ValueError("Port number must be between 1 and 65535.")
            
            # if self.ros_client:
            #     self.ros_client.close()

            self.ros_client = ROS_Client(server_ip=self.host, server_port=port)
            self.ros_client.connect()
        
        except Exception as e:
            print(f"on_pushButton_connect_clicked error: {e}")

    ### TCP断开按钮响应
    def on_pushButton_disconnect_clicked(self):
        try:
            if self.ros_client:
                self.ros_client.close()
        
        except Exception as e:
            print(f"on_pushButton_disconnect_clicked error: {e}")


class Dashboard(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        self._gear = "D"
        self._speed = 0
        self._temperature = 25  # 默认温度值
        self._battery_level = 100  # 默认电池电量百分比

    def set_gear(self, gear):
        self._gear = gear
        self.update()

    def set_speed(self, speed):
        self._speed = speed
        self.update()

    def set_temperature(self, temperature):
        self._temperature = temperature
        self.update()

    def set_battery_level(self, battery_level):
        self._battery_level = max(0, min(100, battery_level))  # 确保电池水平在0到100之间
        self.update()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)

        width = self.width()
        height = self.height()
        margin = 10  # 减少边距以适应更小的空间
        
        # Speedometer is placed in the center
        # speedometer_rect = QRectF(margin, margin, (width - 2 * margin), (height - 4 * margin) / 4)

        # Gear indicator is placed below the speedometer
        # gear_rect = QRectF(margin, speedometer_rect.bottom() + margin, (width - 2 * margin), 40)
        
        # # Temperature meter and Battery meter are placed at the bottom corners with labels above them
        # thermometer_rect = QRectF(margin, height - 30 , (width - 4 * margin) / 2, 105)
        # battery_meter_rect = QRectF(width / 2 + margin, height - 20 , (width - 4 * margin) / 2, 100)

        speedometer_rect = QRectF(margin, margin, (width - 2 * margin), 40)
        gear_rect = QRectF(margin, margin + 60, (width - 2 * margin), 40)
        thermometer_rect = QRectF(margin, margin + 220, (width - 4 * margin) / 2, 205)
        battery_meter_rect = QRectF(width / 2 + margin, margin + 235 , (width - 4 * margin) / 2, 200)

        self.draw_speedometer(painter, speedometer_rect)
        self.draw_gear(painter, gear_rect)
        self.draw_thermometer(painter, thermometer_rect)
        self.draw_battery_meter(painter, battery_meter_rect)

    def draw_speedometer(self, painter, rect):
        painter.save()
        
        # 设置背景线性渐变色
        gradient = QLinearGradient(rect.topLeft(), rect.bottomRight())
        gradient.setColorAt(0, QColor(70, 70, 70))
        gradient.setColorAt(1, QColor(50, 50, 50))
        painter.setBrush(gradient)
        painter.setPen(Qt.NoPen)
        
        # 绘制矩形框代替原来的椭圆
        painter.drawRect(rect)

        # 设置字体和颜色，并绘制文本
        font = QFont("Arial", 8, QFont.Bold)
        painter.setFont(font)
        painter.setPen(Qt.white)
        
        # 使用 QTextOption 来控制文本对齐方式
        text_option = QTextOption(Qt.AlignCenter)
        
        # 绘制速度值，确保文本在矩形框内居中显示
        painter.drawText(rect, f"速度： {self._speed} km/h", text_option)
        
        painter.restore()

    def draw_gear(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.white, 2))
        painter.setBrush(QColor(70, 70, 70))
        painter.drawRect(rect)

        font = QFont("Arial", 8, QFont.Bold)
        painter.setFont(font)
        painter.drawText(rect, Qt.AlignCenter, f"档位: {self._gear}")
        painter.restore()

    def draw_thermometer(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.red, 2))
        painter.setBrush(QColor(255, 0, 0, 127))
        # fill_height = rect.height() * (self._temperature / 100)
        fill_height = 150
        painter.drawRect(QRectF(rect.left(), rect.bottom() - fill_height, rect.width(), -fill_height))

        # Display temperature value above the thermometer with enough space for two lines of text
        label_rect = QRectF(rect.left(), rect.top() - 40, rect.width(), 40)  # Increase height to accommodate two lines
        painter.setPen(Qt.black)

        # Optionally, use QTextOption to control text alignment and wrapping
        text_option = QTextOption()
        text_option.setAlignment(Qt.AlignCenter)
        text_option.setWrapMode(QTextOption.WordWrap)  # Enable word wrap

        painter.drawText(label_rect, "温度\n{:.1f}°C".format(self._temperature), text_option)
        painter.restore()

    def draw_battery_meter(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.blue, 2))
        painter.setBrush(QColor(0, 255, 0, 127))  # Green for battery level
        # fill_height = rect.height() * (self._battery_level / 100)
        fill_height = 150
        painter.drawRect(QRectF(rect.left(), rect.bottom() - fill_height-5, rect.width(), -fill_height))

        # Display battery percentage above the battery meter with enough space for two lines of text
        label_rect = QRectF(rect.left(), rect.top() - 40, rect.width(), 40)  # Increase height to accommodate two lines
        painter.setPen(Qt.black)

        # Optionally, use QTextOption to control text alignment and wrapping
        text_option = QTextOption()
        text_option.setAlignment(Qt.AlignCenter)
        text_option.setWrapMode(QTextOption.WordWrap)  # Enable word wrap

        painter.drawText(label_rect, "电量\n{:.1f}%".format(self._battery_level), text_option)
        painter.restore()

    def resizeEvent(self, event):
        super().resizeEvent(event)
        self.update()

def signal_handler(sig, frame):
    QApplication.quit()

if __name__ == "__main__":
    app = QApplication(sys.argv)
    mainWindow = MainWindow()
    mainWindow.setWindowFlags(
        mainWindow.windowFlags() & ~Qt.WindowMaximizeButtonHint
    )
    mainWindow.show()
    signal.signal(signal.SIGINT, signal_handler)
    sys.exit(app.exec_())